import numpy as np
import cv2
import time
import math


# 得到向量的坐标以及向量的模长
class Point(object):
    def __init__(self, cx, cy, cx3, cy3):
        self.cx = cx
        self.cy = cy
        self.cx3 = cx3
        self.cy3 = cy3

    def vector(self):
        c = (self.cx - self.cx3, self.cy - self.cy3)
        return c

    def length(self):
        d = math.sqrt(pow((self.cx - self.cx3), 2) + pow((self.cy - self.cy3), 2))
        return d


# 计算向量夹角
class Calculate(object):
    def __init__(self, x, y, m, n):
        self.x = x
        self.y = y
        self.m = m
        self.n = n

    def Vector_multiplication(self):
        self.mu = np.dot(self.x, self.y)
        return self.mu

    def Vector_model(self):
        self.de = self.m * self.n
        return self.de

    def cal(self):
        result = Calculate.Vector_multiplication(self) / Calculate.Vector_model(self)
        return int(math.acos(result) / math.pi * 180)


class Detection(object):
    '''
        contours:存放每个轮廓的信息
    '''

    def __init__(self):
        self.contours = []

    def detecte(self, image):
        '''提取所有轮廓'''
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)  # 灰度图
        cv2.imwrite("gray.jpg",gray)
        _, gray = cv2.threshold(gray, 250, 255, cv2.THRESH_OTSU)  # 二值膨胀
        cv2.imwrite("THRESH_OTSU.jpg",gray)


        # 提取特定颜色的二值膨胀
        # hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)  # 色彩空间转换为hsv，便于分离
        # lower_hsv = np.array([37, 43, 46])  # 提取颜色的低值
        # high_hsv = np.array([230, 230, 230])  # 提取颜色的高值
        # # lower_hsv = np.array([156, 43, 46])
        # # high_hsv = np.array([179, 255, 255])
        # gray = cv2.inRange(hsv, lowerb=lower_hsv, upperb=high_hsv)  #小于lowerb与大于upperb的置为0
        con_time = time.time()
        self.contours, hierachy = cv2.findContours(gray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        findContours = cv2.drawContours(gray, self.contours, -1, (0, 100, 100), 3)
        cv2.imwrite("findContours.jpg", findContours)

        # print("time:{}".format(time.time()-con_time))
        # cv2.imshow("1", gray)
        # cv2.waitKey(0)
        return image, self.contours, hierachy

    def compute_1(self, i, j):
        '''回字三个轮廓比例为7：5：3，面积比为49：25：9'''
        '''最外面的轮廓和子轮廓的比例'''
        area1 = cv2.contourArea(self.contours[i])
        area2 = cv2.contourArea(self.contours[j])
        if area2 == 0:
            return False
        ratio = area1 * 1.0 / area2
        if abs(ratio - 49.0 / 25) > 1:
            return False
        return True

    def compute_2(self, i, j):
        '''回字三个轮廓比例为7：5：3，面积比为49：25：9'''
        '''子轮廓和子子轮廓的比例'''
        area1 = cv2.contourArea(self.contours[i])
        area2 = cv2.contourArea(self.contours[j])
        if area2 == 0:
            return False
        ratio = area1 * 1.0 / area2
        if abs(ratio - 25.0 / 9) > 1:

            return False

        return True

    def compute_3(self, i, j):
        '''外轮廓比例为4：3，面积比为16：9'''
        '''子轮廓和子子轮廓的比例'''
        area1 = cv2.contourArea(self.contours[i])
        area2 = cv2.contourArea(self.contours[j])

        if area2 == 0:
            return False
        ratio = area1 * 1.0 / area2
        if abs(ratio - 16.0 / 9) > 0.2 :
            # print(area1, area2)
            return False
        return True

    def compute_center(self, i):
        '''计算轮廓中心点'''
        M = cv2.moments(self.contours[i])
        if M['m00'] == 0:
            return 0, 0
        cx = int(M['m10'] / M['m00'])
        cy = int(M['m01'] / M['m00'])
        return cx, cy

    def detect_contours(self, vec):
        '''判断这个轮廓和它的子轮廓以及子子轮廓的中心的间距是否足够小'''
        distance_1 = np.sqrt((vec[0] - vec[2]) ** 2 + (vec[1] - vec[3]) ** 2)
        distance_2 = np.sqrt((vec[0] - vec[4]) ** 2 + (vec[1] - vec[5]) ** 2)
        distance_3 = np.sqrt((vec[2] - vec[4]) ** 2 + (vec[3] - vec[5]) ** 2)
        if sum((distance_1, distance_2, distance_3)) / 3 < 3:
            return True
        return False

    def detect_center(self, cx, cy, cx1, cy1):
        '''判断是不是两个中心点间距是否足够小'''
        distance_1 = np.sqrt((cx - cx1) ** 2 + (cy - cy1) ** 2)
        if distance_1 > 5:
            return True
        return False


def speed_fliter(d_xy, half_width_height):
    speed_weight_temp = round(d_xy / half_width_height, 2)
    print(speed_weight_temp)
    speed_weight = speed_weight_temp if abs(speed_weight_temp) > 0.2 else 0.05
    return speed_weight


def find(image):
    '''找到符合要求的轮廓'''

    Contour = Detection()
    c_index = 0
    image, contours, hierachy = Contour.detecte(image)
    hierachy = np.squeeze(hierachy)
    rec = []
    outer = []
    Dire = 0
    for i in range(len(hierachy)):
        child = hierachy[i][2]
        child_child = hierachy[child][2]
        child_child_child = hierachy[child_child][2]
        if child != -1 and hierachy[child][2] != -1:
            if child_child_child != -1 and hierachy[child_child_child][2] != -1:
                if Contour.compute_1(child, child_child) and Contour.compute_2(child_child, child_child_child):
                    cx1, cy1 = Contour.compute_center(i)
                    cx2, cy2 = Contour.compute_center(child)
                    cx3, cy3 = Contour.compute_center(child_child)
                    if Contour.detect_contours([cx1, cy1, cx2, cy2, cx3, cy3]):
                        rec.append([cx1, cy1, cx2, cy2, cx3, cy3, i, child, child_child])
                elif Contour.compute_3(i, child_child):#方框比例4:3'''检测kt板大外框'''
                    cx1, cy1 = Contour.compute_center(i)
                    cx2, cy2 = Contour.compute_center(child)
                    cx3, cy3 = Contour.compute_center(child_child)
                    if Contour.detect_contours([cx1, cy1, cx2, cy2, cx3, cy3]):
                        outer = [cx1, cy1, cx2, cy2, cx3, cy3, i, child, child_child]
                        break

    '''检测到大外框的时候，直接用大外框的的中心点'''
    if outer:
        cxz, cyz = Contour.compute_center(outer[6])
        cv2.drawContours(image, contours, outer[6], (0, 0, 255), 4)
        x, y, w, h = cv2.boundingRect(contours[i])
        print(x,y,w,h)
        # cv2.line(image, (x, y), (x+w, y+h), (255, 0, 0), 2)
        cx1 = cxz
        cy1 = cyz - 3
        Dire = 0

        return cxz, cyz, cx1, cy1, Dire, 0

    '''计算得到所有在比例上符合要求的轮廓中心点，用juge_angle判断是否是三角形'''
    i, j, k = juge_angle(rec)
    if i == -1 or j == -1 or k == -1:
        return None
    '''再遍历一遍，把i、j、k三点去除。'''
    for r in range(len(hierachy)):
        child = hierachy[r][2]
        child_child = hierachy[child][2]
        if child != -1 and hierachy[child][2] != -1:
            if Contour.compute_1(r, child) and Contour.compute_2(child, child_child):
                cxz, cyz = Contour.compute_center(r)
                cx2, cy2 = Contour.compute_center(child)
                cx3, cy3 = Contour.compute_center(child_child)
                if Contour.detect_contours([cxz, cyz, cx2, cy2, cx3, cy3]):
                    cxi, cyi = Contour.compute_center(rec[i][6])  # i点的中心坐标，要与r点中心比较
                    cxj, cyj = Contour.compute_center(rec[j][6])  # j点的中心坐标，要与r点中心比较
                    cxk, cyk = Contour.compute_center(rec[k][6])  # k点的中心坐标，要与r点中心比较
                    '''确保r中心点不与i、j、k的中心点重合'''
                    if Contour.detect_center(cxi, cyi, cxz, cyz) and Contour.detect_center(cxj, cyj, cxz,
                                                                                           cyz) and Contour.detect_center(
                        cxk, cyk, cxz, cyz):
                        c_index = r

    '''得到i j k三个点，进行标注画线等操作; (cxz,cyz)为三角形中心的回字的中心坐标'''
    # 计算i、j、k的中心坐标
    if c_index:
        cxz, cyz = Contour.compute_center(c_index)
    cx1, cy1 = Contour.compute_center(rec[i][6])
    cx2, cy2 = Contour.compute_center(rec[j][6])
    cx3, cy3 = Contour.compute_center(rec[k][6])
    cx = int((abs(cx2) + abs(cx3)) / 2)
    cy = int((abs(cy2) + abs(cy3)) / 2)
    cxz1 = int((abs(cx1) + abs(cx)) / 2) #cxz1,cyz1是根据三角形中心线计算出来的中心点，与中间回字的中心点(cxz,cyz)比较
    cyz1 = int((abs(cy1) + abs(cy)) / 2)
    '''不同中心点返回True同个中心点返回False'''
    if Contour.detect_center(cxz, cyz, cxz1, cyz1):
        return None  #中线中心与基准点中心不重合，不是三角形，返回None
    cv2.drawContours(image, contours, c_index, (0, 255, 0), 2)
    cv2.drawContours(image, contours, rec[i][6], (255, 0, 0), 2)
    cv2.drawContours(image, contours, rec[j][6], (255, 0, 0), 2)
    cv2.drawContours(image, contours, rec[k][6], (255, 0, 0), 2)
    cv2.line(image, (cx, cy), (cx1, cy1), (0, 0, 255), 2)  # 要的线
    cv2.line(image, (cx1, cy1), (cx2, cy2), (255, 0, 0), 2)
    cv2.line(image, (cx1, cy1), (cx3, cy3), (255, 0, 0), 2)
    cv2.line(image, (cx2, cy2), (cx3, cy3), (255, 0, 0), 2)
    # distance between c2 and c3
    distance_c2_c3 = math.sqrt((cx3 - cx2) ** 2 + (cy3 - cy2) ** 2)

    print("distance_cx2_cx3:", math.sqrt((cx3 - cx2) ** 2 + (cy3 - cy2) ** 2))
    '''计算非等腰边的中心点'''
    if cx > cx1:
        Dire = -1
    elif cx < cx1:
        Dire = 1
    return cxz, cyz, cx1, cy1, Dire, distance_c2_c3


def juge_angle(rec):
    '''判断寻找是否有三个点可以围成等腰直角三角形'''
    if len(rec) < 3:
        return -1, -1, -1
    for i in range(len(rec)):
        for j in range(i + 1, len(rec)):
            for k in range(j + 1, len(rec)):
                # 三个点之间的距离
                distance_1 = np.sqrt((rec[i][0] - rec[j][0]) ** 2 + (rec[i][1] - rec[j][1]) ** 2)
                distance_2 = np.sqrt((rec[i][0] - rec[k][0]) ** 2 + (rec[i][1] - rec[k][1]) ** 2)
                distance_3 = np.sqrt((rec[j][0] - rec[k][0]) ** 2 + (rec[j][1] - rec[k][1]) ** 2)
                if abs(distance_1 - distance_2) < 10:  # 两边相减小于15（15为允许的误差），则认为两条边相等。
                    # if abs(np.sqrt(np.square(distance_1) + np.square(distance_2)) - distance_3) < 10:   #判断是否是直角
                    return i, j, k
                elif abs(distance_1 - distance_3) < 10:
                    # if abs(np.sqrt(np.square(distance_1) + np.square(distance_3)) - distance_2) < 10:
                    return j, i, k
                elif abs(distance_2 - distance_3) < 10:
                    # if abs(np.sqrt(np.square(distance_2) + np.square(distance_3)) - distance_1) < 10:
                    return k, i, j
    return -1, -1, -1


def camera(path):
    value = []
    capture = cv2.VideoCapture(path)
    Video_FPS = capture.get(cv2.CAP_PROP_FPS)
    height = int(capture.get(cv2.CAP_PROP_FRAME_HEIGHT))
    width = int(capture.get(cv2.CAP_PROP_FRAME_WIDTH))

    # video = cv2.VideoWriter('./video/' + "p2.avi", cv2.VideoWriter_fourcc('D', 'I','V','X'), Video_FPS,
    # (width,height))
    video = cv2.VideoWriter('./video/' + "p4.mp4", cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), Video_FPS,
                            (width, height))
    (np.int(width), np.int(height), True)
    while True:
        ret, image = capture.read()
        if ret is True:
            # 镜像
            start = time.time()
            t1 = int((width / 2) / 2)
            t2 = int((height / 2) / 2)
            image = image[t2:-t2, t1:-t1]
            image = cv2.flip(image, 1)
            width, height = image.shape[1], image.shape[0]
            res = find(image)
            if not res:
                pass
            else:
                cx, cy, cx1, cy1, Dire = res
                first_point = Point(cx, cy, cx1, cy1)
                two_point = Point((width) / 2, height, (width) / 2, 0)
                ca = Calculate(first_point.vector(), two_point.vector(), first_point.length(), two_point.length())
                dx = cx - (width) / 2  # 目标中心点与屏幕中心点差值
                dy = cy - (height) / 2
                value = [dx, dy, Dire, ca.cal()]
                text = "dx:" + str(dx) + " dy:" + str(dy) + ' angle(' + Dire + "):" + str(ca.cal())
                cv2.line(image, (cx, cy), (int(width / 2), int(height / 2)), (0, 0, 255), 2)  # 目标中心点与屏幕中心点连线
                cv2.line(image, (0, int(height / 2)), (width, int(height / 2)), (0, 255, 0), 2)  # 屏幕中心交叉线
                cv2.line(image, (int(width / 2), 0), (int(width / 2), height), (0, 255, 0), 2)  # 屏幕中心交叉线
                cv2.putText(image, text, (20, 40), 1, 4, (0, 255, 0), 4)
                # print(value)
            # video.write(image)
            cv2.imshow('img', image)
            print(int(1 / (time.time() - start)))
            if cv2.waitKey(1) == 27:
                break  # esc to quit
    video.release()
    cv2.destroyAllWindows()


def single_image(image_byte):
    image = cv2.imdecode(np.array(bytearray(image_byte), dtype='uint8'), cv2.IMREAD_UNCHANGED)
    width, height = image.shape[1], image.shape[0]
    half_width = width / 2
    half_height = height / 2
    # print(width,height)
    res = find(image)

    if not res:
        cv2.imshow('sb', image)
        cv2.waitKey(1)
        return 'null'
    else:
        cx, cy, cx1, cy1, Dire, dis_c2_c3 = res
        if dis_c2_c3 > 230:
            return "land"
        first_point = Point(cx, cy, cx1, cy1)
        two_point = Point(half_width, height, half_height, 0)
        ca = Calculate(first_point.vector(), two_point.vector(), first_point.length(), two_point.length())
        dx = cx - half_width
        dy = half_height - cy
        # value = [round(dx/half_width, 2), round(dy/half_height, 2), Dire]
        # 过滤飞行速度，当与降落中心距离足够近时，把飞行速度置0,同时控制无人机下降
        mpitch = speed_fliter(d_xy=dy, half_width_height=half_height)
        mroll = speed_fliter(d_xy=dx, half_width_height=half_width)
        mthrottle = -1 if mpitch == mroll == 0.05 else 0
        value = {"mpitch": mpitch, "mroll": mroll, "myaw": Dire, "mthrottle": mthrottle}
        print(value)

        # visualize the QR code detection
        text = "dx:" + str(dx) + " dy:" + str(dy) + ' angle(' + str(Dire) + "):" + str(ca.cal())
        cv2.line(image, (cx, cy), (int(width / 2), int(height / 2)), (0, 0, 255), 2)  # 目标中心点与屏幕中心点连线
        cv2.line(image, (0, int(height / 2)), (width, int(height / 2)), (0, 255, 0), 2)  # 屏幕中心交叉线
        cv2.line(image, (int(width / 2), 0), (int(width / 2), height), (0, 255, 0), 2)  # 屏幕中心交叉线
        cv2.putText(image, text, (20, 40), 1, 4, (0, 255, 0), 4)
        # cv2.putText(image, text, (20, 30), 1, 1, (0, 255, 0), 2)
        cv2.imshow('sb', image)
        cv2.waitKey(1)
        return value

def single_image_path(image_path):
    cv2.resizeWindow("sb", 1280, 720);
    # image = cv2.imdecode(np.array(bytearray(image_byte), dtype='uint8'), cv2.IMREAD_UNCHANGED)
    image = cv2.imread(image_path)
    width, height = image.shape[1], image.shape[0]
    half_width = width / 2
    half_height = height / 2
    # print(width,height)
    res = find(image)

    if not res:
        cv2.imshow('sb', image)
        cv2.waitKey(0)
        return 'null'
    else:
        cx, cy, cx1, cy1, Dire = res
        first_point = Point(cx, cy, cx1, cy1)
        two_point = Point(half_width, height, half_height, 0)
        ca = Calculate(first_point.vector(), two_point.vector(), first_point.length(), two_point.length())
        dx = cx - half_width
        dy = half_height - cy
        # value = [round(dx/half_width, 2), round(dy/half_height, 2), Dire]
        # 过滤飞行速度，当与降落中心距离足够近时，把飞行速度置0,同时控制无人机下降
        mpitch = speed_fliter(d_xy=dy, half_width_height=half_height)
        mroll = speed_fliter(d_xy=dx, half_width_height=half_width)
        mthrottle = 0 if mpitch == mroll == 0 else -1
        value = {"mpitch": mpitch, "mroll": mroll, "myaw": Dire, "mthrottle": mthrottle}
        print(value)

        # visualize the QR code detection
        text = "dx:" + str(dx) + " dy:" + str(dy) + ' angle(' + str(Dire) + "):" + str(ca.cal())
        cv2.line(image, (cx, cy), (int(width / 2), int(height / 2)), (0, 0, 255), 2)  # 目标中心点与屏幕中心点连线
        cv2.line(image, (0, int(height / 2)), (width, int(height / 2)), (0, 255, 0), 2)  # 屏幕中心交叉线
        cv2.line(image, (int(width / 2), 0), (int(width / 2), height), (0, 255, 0), 2)  # 屏幕中心交叉线
        cv2.putText(image, text, (20, 40), 1, 4, (0, 255, 0), 4)
        # cv2.putText(image, text, (20, 30), 1, 1, (0, 255, 0), 2)
        cv2.imshow('sb', image)
        cv2.waitKey(0)
        return value



if __name__ == '__main__':
    path = "./image/v2.png"
    path1 = "QRcode.jpg"
    path2 = "./FlaskServer/static/images/QRcode.jpg"

    single_image_path(path1)
    # camera(path1)#打开视频
    # camera(0)#0为调用摄像头
